﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.MotionPlanning.Primitives;

namespace Drifting.MotionPlanning.PointSets
{
    public class LinearQuery : PointSet
    {
        protected List<State> pointList;

        public LinearQuery()
        {
            pointList = new List<State>(10240);
        }

        override public bool AddState(State x)
        {
            lock (pointList)
            {
                pointList.Add(x);
                return true;
            }
        }

        override public void Neighbor_SingleQuery(double[] q, ref State x_nearest, ref double dist2)
        {
            double dist_min = double.MaxValue;
            State x_min = null;
            foreach (State x_i in pointList)
            {
                double dist_i = Euclidean2(x_i.q, q);
                if (dist_i < dist_min)
                {
                    dist_min = dist_i;
                    x_min = x_i;
                }
            }
            x_nearest = x_min;
            dist2 = dist_min;
        }

        override public void ReportVerticesSnapshot(ref List<State> snapshot)
        {
            throw new NotImplementedException();
            /*
            if (snapshot == null)
                snapshot = new List<float[]>(pointList.Count);
            int i = 0;
            while (i < pointList.Count)
            {
                State x_i = pointList[i];
                double[] q_i = x_i.q;

                float[] fq_i = null;

                if (snapshot.Count > i)
                    fq_i = snapshot[i];
                else
                {
                    fq_i = new float[6];
                    snapshot.Add(fq_i);
                }

                fq_i[0] = (float)q_i[0];
                fq_i[1] = (float)q_i[1];
                if (x_i.q.Length < 3)
                    fq_i[2] = 0;
                else
                    fq_i[2] = (float)q_i[2];

                if (x_i.parentEdge != null)
                {
                    double[] q_p = x_i.parentEdge.entry.q;

                    fq_i[3] = (float)q_p[0];
                    fq_i[4] = (float)q_p[1];
                    if (x_i.q.Length < 3)
                        fq_i[5] = 0;
                    else
                        fq_i[5] = (float)q_p[3];
                }
                i++;
            }
             * */
        }
    }
}
